#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include <iostream>

using namespace std;
class Teleop //定义一个类
{
    public:
        Teleop();
    private:
        void callback(const sensor_msgs::Joy::ConstPtr& Joy);
        ros::NodeHandle n; //实例化节点
        ros::Subscriber sub; //声明一个订阅对象
        ros::Publisher pub; //声明一个发布对象
        double vlinear, vangular; //我们控制乌龟的速度，是通过这两个变量调整
        int axis_ang, axis_lin; //axis[]的键
};

Teleop::Teleop()
{
    n.param<int>("axis_linear", axis_lin, 1); //定义参数摇杆1接受线速度
    n.param<int>("axis_angular", axis_ang, 0); //定义参数摇杆2接受角速度
    n.param<double>("vel_linear", vlinear, 1); //定义参数vlinear为1，单位m/s
    n.param<double>("vel_angular", vangular, 1); //定义参数vangular为1，单位rad/s
    pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 1); //将速度发给乌龟
    sub = n.subscribe<sensor_msgs::Joy>("joy", 10, &Teleop::callback, this); //订阅游戏手柄发来的数据   
    // param 1:设置订阅话题的名称
    // param 2:接受缓冲区数据大小
    // param 3:接收到函数后需操作的调用函数
}

void Teleop::callback(const sensor_msgs::Joy::ConstPtr& Joy) //Teleop具体内容
{
    geometry_msgs::Twist v; //声明变量，类型为Twist
    v.linear.x = Joy->axes[axis_lin]*vlinear; //将游戏手柄的数据乘以线速度的值，然后发给乌龟
    v.angular.z = Joy->axes[axis_ang]*vangular; //将游戏手柄数据乘以角速度的值，然后发给乌龟
    ROS_INFO("linear:%.3If angular:%.3If", v.linear.x, v.angular.z);
    pub.publish(v); //发布话题
}

int main(int argc, char** argv) //主函数
{
    ros::init(argc, argv, "teleopjoy");
    Teleop teleop_turtle;
    ros::spin();
    return 0;
}
